using BEPUphysics;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Input;
using System;
using System.Diagnostics;
using BEPUphysics.Collidables.MobileCollidables;
using BEPUphysics.Entities;
using PerformanceUtility.GameDebugTools;
using EventFlowLibrary.GameStructure;

namespace LevelComponents.Character
{
    /// <summary>
    /// Handles input and movement of a character in the game.
    /// Acts as a simple 'front end' for the bookkeeping and math of the character controller.
    /// </summary>
    public class CharacterControllerInput
    {
        /// <summary>
        /// Camera to use for input.
        /// </summary>
        public Camera Camera;

        /// <summary>
        /// Offset from the position of the character to the 'eyes' while the character is standing.
        /// </summary>
        public const float StandingCameraOffset = 1.7f;

        /// <summary>
        /// Offset from the position of the character to the 'eyes' while the character is crouching.
        /// </summary>
        public const float CrouchingCameraOffset = 1.0f;

        /// <summary>
        /// Physics representation of the character.
        /// </summary>
        public CharacterController CharacterController;

        /// <summary>
        /// Whether or not to smooth out character steps and other discontinuous motion.
        /// </summary>
        public const bool UseCameraSmoothing = true;

        /// <summary>
        /// Owning space of the character.
        /// </summary>
        public Space Space { get; private set; }

        /// <summary>
        /// How our model has rotated on the YAxis
        /// </summary>
        public float totalRotationY = 0.0f;

        /// <summary>
        /// The direction in which we are moving.
        /// </summary>
        private Vector3 movementDir = Vector3.Zero;

        /// <summary>
        /// Our total character movement
        /// </summary>
        Vector2 totalMovement = Vector2.Zero;

        /// <summary>
        /// Constructs the character and internal physics character controller.
        /// </summary>
        /// <param name="owningSpace">Space to add the character to.</param>
        /// <param name="CameraToUse">Camera to attach to the character.</param>
        public CharacterControllerInput(Space owningSpace, Camera CameraToUse, Vector3 _pos, float _height, float _radius, float _mass)
        {
            CharacterController = new CharacterController(_pos, _height, _radius, _mass);

            Space = owningSpace;
            Space.Add(CharacterController);

            Camera = CameraToUse;
            totalRotationY = Camera.Yaw;
            CharacterController.Body.Position = (Camera.Position - new Vector3(0, StandingCameraOffset, 0));
        }

        /// <summary>
        /// Handles the input and movement of the character.
        /// </summary>
        /// <param name="dt">Time since last frame in simulation seconds.</param>
        /// <param name="previousKeyboardInput">The last frame's keyboard state.</param>
        /// <param name="keyboardInput">The current frame's keyboard state.</param>
        /// <param name="previousGamePadInput">The last frame's gamepad state.</param>
        /// <param name="gamePadInput">The current frame's keyboard state.</param>
        public void Update(float dt, KeyboardState previousKeyboardInput, KeyboardState keyboardInput, GamePadState previousGamePadInput, GamePadState gamePadInput)
        {
            //DebugSystem.Instance.EventDisplayer.EnqueueDisplayEvent(string.Format("dt : {0}", dt.ToString()));

            // James: disabled, seems smoother
            //Note that the character controller's update method is not called here; this is because it is handled within its owning space.
            //This method's job is simply to tell the character to move around based on the Camera and input.
            //if (Camera.Mode != CameraMode.Manual)
           // {
                ////First, find where the camera is expected to be based on the last position and the current velocity.
                ////Note: if the character were a free-floating 6DOF character, this would need to include an angular velocity contribution.
                ////And of course, the camera orientation would be based on the character's orientation.
                //Camera.Position = Camera.Position + CharacterController.Body.LinearVelocity * dt;
                ////Now compute where it should be according the physical body of the character.
                //Vector3 up = CharacterController.Body.OrientationMatrix.Up;
                //Vector3 bodyPosition = CharacterController.Body.Position;
                //Vector3 goalPosition = bodyPosition + up * (CharacterController.StanceManager.CurrentStance == Stance.Standing ? StandingCameraOffset : CrouchingCameraOffset);

                ////Usually, the camera position and the goal will be very close, if not matching completely.
                ////However, if the character steps or has its position otherwise modified, then they will not match.
                ////In this case, we need to correct the camera position.

                ////To do this, first note that we can't correct infinite errors.  We need to define a bounding region that is relative to the character
                ////in which the camera can interpolate around.  The most common discontinuous motions are those of upstepping and downstepping.
                ////In downstepping, the character can teleport up to the character's MaximumStepHeight downwards.
                ////In upstepping, the character can teleport up to the character's MaximumStepHeight upwards, and the body's CollisionMargin horizontally.
                ////Picking those as bounds creates a constraining cylinder.

                //Vector3 error = goalPosition - Camera.Position;
                //float verticalError = Vector3.Dot(error, up);
                //Vector3 horizontalError = error - verticalError * up;
                ////Clamp the vertical component of the camera position within the bounding cylinder.
                //if (verticalError > CharacterController.StepManager.MaximumStepHeight)
                //{
                //    Camera.Position -= up * (CharacterController.StepManager.MaximumStepHeight - verticalError);
                //    verticalError = CharacterController.StepManager.MaximumStepHeight;
                //}
                //else if (verticalError < -CharacterController.StepManager.MaximumStepHeight)
                //{
                //    Camera.Position -= up * (-CharacterController.StepManager.MaximumStepHeight - verticalError);
                //    verticalError = -CharacterController.StepManager.MaximumStepHeight;
                //}
                ////Clamp the horizontal distance too.
                //float horizontalErrorLength = horizontalError.LengthSquared();
                //float margin = CharacterController.Body.CollisionInformation.Shape.CollisionMargin;
                //if (horizontalErrorLength > margin * margin)
                //{
                //    Vector3 previousHorizontalError = horizontalError;
                //    Vector3.Multiply(ref horizontalError, margin / (float)Math.Sqrt(horizontalErrorLength), out horizontalError);
                //    Camera.Position -= horizontalError - previousHorizontalError;
                //    horizontalErrorLength = margin * margin;
                //}
                ////Now that the error/camera position is known to lie within the constraining cylinder, we can perform a smooth correction.

                ////This removes a portion of the error each frame.
                ////Note that this is not framerate independent.  If fixed time step is not enabled,
                ////a different smoothing method should be applied to the final error values.
                ////float errorCorrectionFactor = .3f;

                ////This version is framerate independent, although it is more expensive.
                //float errorCorrectionFactor = (float)(1 - Math.Pow(.000000001, dt));
                //Camera.Position += up * (verticalError * errorCorrectionFactor);
                //Camera.Position += horizontalError * errorCorrectionFactor;
            //}

            HandleInput(dt, previousKeyboardInput, keyboardInput, previousGamePadInput, gamePadInput);
        }

        /// <summary>
        /// Handle input to the character
        /// </summary>
        /// <param name="previousKeyboardInput"></param>
        /// <param name="keyboardInput"></param>
        /// <param name="previousGamePadInput"></param>
        /// <param name="gamePadInput"></param>
        private void HandleInput(float dt, KeyboardState previousKeyboardInput, KeyboardState keyboardInput, GamePadState previousGamePadInput, GamePadState gamePadInput)
        {
            totalMovement = Vector2.Zero;
            movementDir = Vector3.Zero;

            // Character rotation is not tied to camera rotation in offset.
            //if (Camera.Mode == CameraMode.FirstPersonMovement || Camera.Mode == CameraMode.FirstPersonStatic)
            //{
            //    totalRotationY = Camera.Yaw;
            //}

            if (Camera.Mode == CameraMode.Manual)
            {
                CharacterController.HorizontalMotionConstraint.MovementDirection = Vector2.Zero;
            }

#if XBOX360
            // Collect the movement impulses.
            // If in static mode, just tie to the body position (in case we are sliding/falling/etc)
            if (Camera.Mode != CameraMode.FirstPersonStatic && Camera.Mode != CameraMode.Manual)
            {
                Vector3 forward = Camera.WorldMatrix.Forward;
                forward.Y = 0;
                forward.Normalize();
                Vector3 right = Camera.WorldMatrix.Right;
                totalMovement += gamePadInput.ThumbSticks.Left.Y * new Vector2(forward.X, forward.Z);
                totalMovement += gamePadInput.ThumbSticks.Left.X * new Vector2(right.X, right.Z);
                CharacterController.HorizontalMotionConstraint.MovementDirection = Vector2.Normalize(totalMovement);

                CharacterController.StanceManager.DesiredStance = gamePadInput.IsButtonDown(Buttons.RightStick) ? Stance.Crouching : Stance.Standing;

                //Jumping
                if (previousGamePadInput.IsButtonUp(Buttons.LeftStick) && gamePadInput.IsButtonDown(Buttons.LeftStick))
                {
                    CharacterController.Jump();
                }
            }
            else
            {
                Camera.Position = CharacterController.Body.Position;
            }

#else
            // Collect the movement impulses.
            // If in static mode, just tie to the body position (in case we are sliding/falling/etc)
            if (Camera.Mode != CameraMode.FirstPersonStatic && Camera.Mode != CameraMode.Manual)
            {
                // Forward/Backward
                if (keyboardInput.IsKeyDown(Keys.W))
                {
                    switch (Camera.Mode)
                    {
                        case CameraMode.ChaseSpring:
                        case CameraMode.ChaseStiff:
                            movementDir = CharacterController.Body.OrientationMatrix.Forward; break;
                        case CameraMode.FirstPersonMovement:
                        case CameraMode.FirstPersonStatic:
                        case CameraMode.Offset:
                            movementDir = Camera.WorldMatrix.Forward; break;
                    }

                    totalMovement += Vector2.Normalize(new Vector2(movementDir.X, movementDir.Z));

                }
                else if (keyboardInput.IsKeyDown(Keys.S))
                {
                    switch (Camera.Mode)
                    {
                        case CameraMode.ChaseSpring:
                        case CameraMode.ChaseStiff:
                            movementDir = CharacterController.Body.OrientationMatrix.Forward; break;
                        case CameraMode.FirstPersonMovement:
                        case CameraMode.FirstPersonStatic:
                        case CameraMode.Offset:
                            movementDir = Camera.WorldMatrix.Forward; break;
                    }

                    totalMovement -= Vector2.Normalize(new Vector2(movementDir.X, movementDir.Z));
                }

                // Left Right
                if (keyboardInput.IsKeyDown(Keys.A))
                {
                    switch (Camera.Mode)
                    {
                        case CameraMode.ChaseSpring:
                        case CameraMode.ChaseStiff:
                            movementDir = CharacterController.Body.OrientationMatrix.Left; break;
                        case CameraMode.FirstPersonMovement:
                        case CameraMode.FirstPersonStatic:
                        case CameraMode.Offset:
                            movementDir = Camera.WorldMatrix.Left; break;
                    }

                    totalMovement += Vector2.Normalize(new Vector2(movementDir.X, movementDir.Z));
                }
                else if (keyboardInput.IsKeyDown(Keys.D))
                {
                    switch (Camera.Mode)
                    {
                        case CameraMode.ChaseSpring:
                        case CameraMode.ChaseStiff:
                            movementDir = CharacterController.Body.OrientationMatrix.Right; break;
                        case CameraMode.FirstPersonMovement:
                        case CameraMode.FirstPersonStatic:
                        case CameraMode.Offset:
                            movementDir = Camera.WorldMatrix.Right; break;
                    }

                    totalMovement += Vector2.Normalize(new Vector2(movementDir.X, movementDir.Z));
                }

                //Rotate YAxis
                if (keyboardInput.IsKeyDown(Keys.Q))
                {
                    totalRotationY += 0.01f;

                    if (Camera.Mode != CameraMode.Offset)
                        Camera.Yaw = totalRotationY;

                    while (totalRotationY > MathHelper.TwoPi)
                        totalRotationY -= MathHelper.TwoPi;
                }
                else if (keyboardInput.IsKeyDown(Keys.E))
                {
                    totalRotationY -= 0.01f;

                    if (Camera.Mode != CameraMode.Offset)
                        Camera.Yaw = totalRotationY;

                    while (totalRotationY < 0)
                        totalRotationY += MathHelper.TwoPi;
                }

                if (totalMovement == Vector2.Zero)
                    CharacterController.HorizontalMotionConstraint.MovementDirection = Vector2.Zero;
                else
                    CharacterController.HorizontalMotionConstraint.MovementDirection = Vector2.Normalize(totalMovement);

                CharacterController.StanceManager.DesiredStance = keyboardInput.IsKeyDown(Keys.Z) ? Stance.Crouching : Stance.Standing;

                //Jumping
                if (previousKeyboardInput.IsKeyUp(Keys.Space) && keyboardInput.IsKeyDown(Keys.Space))
                {
                    CharacterController.Jump();
                }
            }
            else if (Camera.Mode != CameraMode.Manual)
            {
                Camera.Position = CharacterController.Body.Position;
            }

            // This is disabled to prevent NaN from occuring in Vector3s. When rotating the body, in some cases when crossing from model to
            // model or hanging over an edge while looking down, an error gets thrown from Bepu. For this game, we are going FPS only, so don't
            // have to worry about character orientation - for future 3rd person cameras, rotate model based on yaw/pitch. How to orient 3D model in bepu?
            //CharacterController.Body.Orientation = Quaternion.CreateFromRotationMatrix(Matrix.CreateRotationY(totalRotationY));

            // Create the bob for a first person camera.
            if (Camera.Mode == CameraMode.FirstPersonMovement && 
                (totalMovement.X != 0 || totalMovement.Y != 0))
                //(CharacterController.Body.LinearVelocity.X > 0.1f || CharacterController.Body.LinearVelocity.X < -0.1f || CharacterController.Body.LinearVelocity.Z > 0.1f || CharacterController.Body.LinearVelocity.Z < -0.1f))
            {
                // Change here to increase rapidity of bounce
                Camera.PreviousCameraBob = Camera.CameraBob;
                Camera.CameraBob += 8f * dt;

                // We check if we are bottom of sin wave to play footstep sounds
                if (Camera.PreviousCameraBob < MathHelper.Pi &&
                    Camera.CameraBob >= MathHelper.Pi)
                {
                    //DebugSystem.Instance.EventDisplayer.EnqueueDisplayEvent(string.Format("Played 1: {0} at {1}", "footstep1", dt));
                    AudioManager.soundEffects["footstep1"].PlaySound(1.0f, 0, 0, false);
                }
                else if (Camera.CameraBob > MathHelper.TwoPi)
                {
                    // Prevent bob from overflowing
                    Camera.CameraBob -= MathHelper.TwoPi;
                }
            }

            // Change the camera mode
            //if (keyboardInput.IsKeyDown(Keys.Enter) && previousKeyboardInput.IsKeyUp(Keys.Enter))
            //{
            //    switch (Camera.Mode)
            //    {
            //        case CameraMode.ChaseSpring:
            //            Camera.SetCameraMode(CameraMode.ChaseStiff, CharacterController.Body, new Vector3(0, 2.0f, 0), true, 15); break;
            //        case CameraMode.ChaseStiff:
            //            Camera.SetCameraMode(CameraMode.FirstPersonMovement, CharacterController.Body, new Vector3(0, 2.0f, 0), true, 15); 
            //            Camera.Yaw = totalRotationY; break;
            //        case CameraMode.FirstPersonMovement:
            //            Camera.SetCameraMode(CameraMode.FirstPersonStatic, CharacterController.Body, new Vector3(0, 2.0f, 0), true, 15);
            //            Camera.Yaw = totalRotationY; break;
            //        case CameraMode.FirstPersonStatic:
            //            Camera.SetCameraMode(CameraMode.Offset, CharacterController.Body, new Vector3(0, 2.0f, 0), true, 15); break;
            //        case CameraMode.Offset:
            //            Camera.SetCameraMode(CameraMode.Manual, CharacterController.Body, new Vector3(0, 2.0f, 0), true, 15);
            //            Camera.InitCurves(CurveLoopType.Oscillate, CurveLoopType.Oscillate, CurveLoopType.Oscillate, CurveLoopType.Oscillate, null, null); break;
            //        case CameraMode.Manual:
            //            Camera.SetCameraMode(CameraMode.ChaseSpring, CharacterController.Body, new Vector3(0, 2.0f, 0), true, 15); break;
            //    }
            //}
#endif
        }
    }
}